{
 "cells": [
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "a33a60be-1c4f-4515-b091-85c87e40be4b",
   "metadata": {},
   "source": [
    "# Robodyno IMU Sensor API"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "125f5f75-af84-44da-9a29-14a950856a90",
   "metadata": {},
   "source": [
    "## 1.初始化IMU对象"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "d15fa1be-065e-43ff-ad96-f1474fdb902b",
   "metadata": {},
   "source": [
    "`ImuSensor(can, id_)`\n",
    "\n",
    "参数：\n",
    "- `can` : can总线接口\n",
    "- `id_` : IMU驱动板模块ID，默认`0x31`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "a878b6fb-9912-419e-bd75-c91c5e5e376b",
   "metadata": {
    "collapsed": true,
    "jupyter": {
     "outputs_hidden": true
    },
    "tags": []
   },
   "outputs": [],
   "source": [
    "from robodyno.components import ImuSensor\n",
    "from robodyno.interfaces import CanBus\n",
    "can = CanBus()\n",
    "imu = ImuSensor(can, 0x31)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "6220f4cb-76bc-49e1-a1cf-a54e9c6441c0",
   "metadata": {},
   "source": [
    "## 2.读取IMU版本"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "e0ce197b-dd36-463c-ac45-5133c86fe758",
   "metadata": {},
   "source": [
    "`get_version(timeout = 0)`\n",
    "\n",
    "参数：\n",
    "\n",
    "- timeout : 请求超时时间(s)，0代表无超时时间\n",
    "\n",
    "返回值 ：\n",
    "\n",
    "- API版本dict\n",
    "    - `main_version` : 主版本号\n",
    "    - `sub_version` : 副版本号\n",
    "    - `type` : 设备类型\n",
    "        - ROBODYNO_IMU_SENSOR"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "8c3f87a0-a132-432b-b22c-1a7dfdfbe5bc",
   "metadata": {
    "tags": []
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "{'main_version': 0, 'sub_version': 1, 'type': <Model.ROBODYNO_IMU_SENSOR: 161>}"
      ]
     },
     "execution_count": 2,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "imu.get_version(1)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "0fb0c80b-82f7-4ee8-adf8-094fc621924c",
   "metadata": {
    "tags": []
   },
   "source": [
    "## 3.四元数转欧拉角"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "173be8eb-1677-4c26-ab9c-9a691d14cc4a",
   "metadata": {},
   "source": [
    "`quat_to_euler(x, y, z, w)`\n",
    "\n",
    "参数：\n",
    "\n",
    "- `x` : 四元数的x值\n",
    "- `y` : 四元数的y值\n",
    "- `z` : 四元数的z值\n",
    "- `w` : 四元数的w值\n",
    "\n",
    "返回值：\n",
    "\n",
    "- 欧拉角的元组\n",
    "    - `roll` : 欧拉角的roll值\n",
    "    - `pitch` : 欧拉角的pitch值\n",
    "    - `yaw` : 欧拉角的yaw值"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "0a928c8e-d602-43aa-a4a0-2722ed306679",
   "metadata": {
    "tags": []
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(1.355383723185592, -0.8135231768248526, -1.803965448606355)"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "imu.quat_to_euler(*imu.get_quaternion())"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "2c531d37-bfc6-4d0b-8194-c5fd3b4f2576",
   "metadata": {},
   "source": [
    "## 4.设置CAN_ID"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "f8b595eb-f325-4d88-be72-28801d55d9ca",
   "metadata": {},
   "source": [
    "`config_can_bus(new_id, heartbeat = 1000, bitrate = 1000000)`\n",
    "\n",
    "参数 ：\n",
    "\n",
    "- `new_id` ：IMU 传感器新CAN_ID (默认：`0x31`)\n",
    "- `heartbeat` : 心跳包发送周期 （ ms ）\n",
    "- `bitrate` : CAN总线通讯速率\n",
    "    - 1000000\n",
    "    - 500000\n",
    "    - 250000"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "2236fa59-4f9c-46a3-871c-8a2ebf6ad948",
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "imu.config_can_bus(0x31)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "c061bc1f-7c9f-41fe-b831-a0d2c79276b0",
   "metadata": {
    "tags": []
   },
   "source": [
    "## 5.设置IMU的量程"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "5902d10c-294b-4d78-b673-9ba2ef306a65",
   "metadata": {},
   "source": [
    "`set_ranges(gyro_range, accel_range)`\n",
    "\n",
    "参数 ：\n",
    "\n",
    "- `gyro_range` ：陀螺仪量程\n",
    "    - 0: 250dps \n",
    "    - 1: 500dps \n",
    "    - 2: 1000dps \n",
    "    - 3: 2000dps\n",
    "- `accel_range` : 加速度量程\n",
    "    - 0: 2g\n",
    "    - 1: 4g\n",
    "    - 2: 8g\n",
    "    - 3: 16g"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "b80092d8-eed4-4167-adc4-cf0c0dbf2b33",
   "metadata": {
    "collapsed": true,
    "jupyter": {
     "outputs_hidden": true
    },
    "tags": []
   },
   "outputs": [],
   "source": [
    "imu.set_ranges(1, 1)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "d9ed41f4-43aa-4bf3-a7a8-228f5e5c21d8",
   "metadata": {},
   "source": [
    "## 6.读取IMU的四元数"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "8571ce8c-32e2-4ad5-8cf3-0f36790d6f2a",
   "metadata": {},
   "source": [
    "`get_quaternion(timeout=None)`\n",
    "\n",
    "参数：\n",
    "\n",
    "- `timeout` : 请求超时时间(s)\n",
    "\n",
    "返回值:\n",
    "\n",
    "- IMU 传感器的四元数(x, y, z, w)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "06c5b473-72ee-4c4b-99da-039c507489ab",
   "metadata": {
    "tags": []
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(-0.30126953125, 0.826171875, 0.35400390625, -0.318115234375)"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "imu.get_quaternion()"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "d8b3f2a2-e69d-4594-b4bc-5ad661233ae0",
   "metadata": {},
   "source": [
    "## 7.读取IMU的欧拉角"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "4da4622c-aba2-4775-9982-01fa788dcc2b",
   "metadata": {},
   "source": [
    "`get_euler(timeout=None)`\n",
    "\n",
    "参数：\n",
    "\n",
    "- `timeout` : 请求超时时间(s)\n",
    "\n",
    "返回值:\n",
    "\n",
    "- IMU 传感器的欧拉角(roll, pitch, yaw)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "d0260943-acba-47fb-957e-ce9593d78b01",
   "metadata": {
    "tags": []
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(2.3297165575967056, -0.16949245470737628, -2.2967352819805584)"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "imu.get_euler()"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "907aaabe-8a37-45de-911e-5dba6add972b",
   "metadata": {
    "tags": []
   },
   "source": [
    "## 8.读取IMU的陀螺仪"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "dfec7643-0247-43cf-b9a2-1e6b96138d9e",
   "metadata": {},
   "source": [
    "`get_gyro(timeout=None)`\n",
    "\n",
    "参数：\n",
    "\n",
    "- `timeout` : 请求超时时间(s)\n",
    "\n",
    "返回值:\n",
    "\n",
    "- IMU 传感器的陀螺仪(gyro_x, gyro_y, gyro_z), 单位：(rad/s)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "6ad0c1ba-f12a-406f-a613-24b4ecbf18df",
   "metadata": {
    "tags": []
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(-0.009321063815277334, -0.008189220351993659, -0.003528688444354991)"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "imu.get_gyro()"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "e8000b66-44f9-4128-8955-13f42abadbd6",
   "metadata": {
    "tags": []
   },
   "source": [
    "## 9.读取IMU的加速度"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "cb74d569-30bc-401e-b00a-0f628659a7d4",
   "metadata": {},
   "source": [
    "`get_accel(timeout=None)`\n",
    "\n",
    "参数：\n",
    "\n",
    "- `timeout` : 请求超时时间(s)\n",
    "\n",
    "返回值:\n",
    "\n",
    "- IMU 传感器的加速度(accel_x, accel_y, accel_z), 单位：(rad/s^2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "b0e3cb3f-2ec8-43a1-a882-e54c03d293d5",
   "metadata": {
    "tags": []
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(0.12868833923339842, -0.17896657409667968, 2.7994202911376953)"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "imu.get_accel()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
